#include "lc3.h"

void lc3_init(lc3machine* state) 
{
	// Initialize the lc3 state according to the assignment.
	state->pc = 0x3000;
	state->cc = LC3_ZERO;
	state->halted = 0;
}

void lc3_load(lc3machine* state, FILE* program)
{
	// Reread the section on the .obj file format.while(write != EOF)
	for(int i = 0; i<65536; i++)
		state->mem[i] = 0;
	for(int i = 0; i<8; i++)
		state->regs[i] = 0;
	unsigned short init = (fgetc(program)<<8) | fgetc(program);
	short write =(fgetc(program)<<8) | fgetc(program);
	do{
		state->mem[init] = write;
		write = (fgetc(program)<<8) | fgetc(program);
		init++;	
	}while(write != EOF);
}

void lc3_step_one(lc3machine* state)
{
	// If the machine is not halted
	// Fetch an instruction
	// And call lc3_execute 
	if(!state->halted){
		lc3_execute(state, lc3_fetch(state));
	}
}

void lc3_run(lc3machine* state, int num_steps)
{
	for(int i = 0; i<num_steps; i++){
		if(state->halted)
			break;
		lc3_execute(state, lc3_fetch(state));
	}
}

unsigned short lc3_fetch(lc3machine* state)
{
	/* Think back to when we first started assembly to code this */
	/* What happens during the fetch stage? */
	unsigned short val = state->mem[state->pc];
	state->pc++;
	return val;
}

void lc3_execute(lc3machine* state, unsigned short instruction){
	unsigned short opcode = instruction>>12;
	switch (opcode){
		case ADD:
			//printf("add op\n");
			lc3_add(state, instruction);
			break;
		case AND:
			//printf("and op\n");
			lc3_and(state,instruction);
			break;
		case BR:
			//printf("br op\n");
			lc3_br(state, instruction);
			break;
		case JMP:
			//printf("jmp op\n");
			lc3_jmp(state, instruction);
			break;
		case JSR:
			//printf("jsr op\n");
			lc3_jsr(state, instruction);
			break;
		case LD:
			//printf("ld op\n");
			lc3_ld(state, instruction);
			break;
		case LDI:
			//printf("ldi op\n");
			lc3_ldi(state, instruction);
			break;
		case LDR:
			//printf("ldr op\n");
			lc3_ldr(state, instruction);
			break;
		case LEA:
			//printf("lea op\n");
			lc3_lea(state, instruction);
			break;
		case NOT:
			//printf("not op\n");
			lc3_not(state, instruction);
			break;
		case ST:
			//printf("st op\n");
			lc3_st(state, instruction);
			break;
		case STI:
			//printf("sti op\n");
			lc3_sti(state, instruction);
			break;
		case STR:
			//printf("str op\n");
			lc3_str(state, instruction);
			break;
		case TRAP:
			//printf("trap op\n");
			lc3_trap(state, (instruction & 0xFF));
			break;
		default:
			break;
	}
}

void lc3_add(lc3machine* state, unsigned short instruction){
	unsigned char DR = (instruction>>9) & 0x7;
	unsigned char SR1 = (instruction>>6) & 0x7;
	if(instruction&32){
		char imm5 = sext(instruction & 0x1F, 5);
		state->regs[DR] = state->regs[SR1]+imm5;
	}
	else{
		unsigned char SR2 = instruction & 0x7;
		//printf("not an imm5!\n");
		state->regs[DR] = state->regs[SR1]+state->regs[SR2];
	}
	setCC(state, DR);
}

void lc3_and(lc3machine* state, unsigned short instruction){
	unsigned char DR = (instruction>>9) & 0x7;
	unsigned char SR1 = (instruction>>6) & 0x7;
	if(instruction&32){
		char imm5 = sext(instruction & 0x1F, 5);
		state->regs[DR] = state->regs[SR1] & imm5;
	}
	else{
		unsigned char SR2 = instruction & 0x7;
		state->regs[DR] = state->regs[SR1] & state->regs[SR2];
	}
	setCC(state, DR);
}

void lc3_br(lc3machine* state, unsigned short instruction){
	short off9 = sext(instruction & 0x1FF, 9);
	if((instruction>>9)&1){
		if(state->cc == LC3_POSITIVE)
			state->pc += off9;
	}
	else if((instruction>>10)&1){
		if(state->cc == LC3_ZERO)
			state->pc += off9;
	}
	else if((instruction>>11)&1){
		if(state->cc == LC3_NEGATIVE)
			state->pc += off9;
	}
}
void lc3_jmp(lc3machine* state, unsigned short instruction){
	state->pc = state->regs[(instruction>>6)&0x7];
}

void lc3_jsr(lc3machine* state, unsigned short instruction){
	if((instruction>>11)&1){
		state->pc += sext((instruction)&0x7FF, 11);
	}
	else{
		//printf("jsrr\n");
		unsigned short pc = state->pc;
		state->pc = state->regs[(instruction>>6)&0x7];
		state->regs[7] = pc;
	}
}
void lc3_ld(lc3machine* state, unsigned short instruction){
	unsigned char DR = (instruction>>9)&0x7;
	state->regs[DR] = state->mem[state->pc + sext(instruction & 0x1FF, 9)];
	setCC(state, DR);
}

void lc3_ldi(lc3machine* state, unsigned short instruction){
	unsigned char DR = (instruction>>9)&0x7;
	state->regs[DR] = state->mem[state->mem[state->pc + sext(instruction & 0x1FF, 9)]];
	setCC(state, DR);
}

void lc3_ldr(lc3machine* state, unsigned short instruction){
	unsigned char DR = (instruction>>9)&0x7;
	unsigned char BaseR = (instruction>>6)&0x7;
	short off6 = sext(instruction&0x3f, 6);
	state->regs[DR] = state->mem[state->regs[BaseR] + off6];
	setCC(state, DR);
}

void lc3_lea(lc3machine* state, unsigned short instruction){
	unsigned char DR = (instruction>>9)&0x7;
	short off9 = sext(instruction & 0x1FF, 9);
	state->regs[DR] = state->pc + off9;
	setCC(state, DR);
}

void lc3_not(lc3machine* state, unsigned short instruction){
	unsigned char DR = (instruction>>9)&0x7;
	unsigned char SR = (instruction>>6) & 0x7;
	state->regs[DR] = ~state->regs[SR];
	setCC(state,DR);
}

void lc3_st(lc3machine* state, unsigned short instruction){
	unsigned char SR = (instruction>>9)&0x7;
	short off9 = sext(instruction & 0x1FF, 9);
	state->mem[state->pc +off9] = state->regs[SR];
}

void lc3_sti(lc3machine* state, unsigned short instruction){
	unsigned char SR = (instruction>>9)&0x7;
	short off9 = sext(instruction & 0x1FF, 9);
	state->mem[state->mem[state->pc + off9]] = state->regs[SR];
}

void lc3_str(lc3machine* state, unsigned short instruction){
	unsigned char SR = (instruction>>9)&0x7;
	unsigned char BaseR = (instruction>>6)&0x7;
	short off6 = sext(instruction&0x3f, 6);
	state->mem[state->regs[BaseR] + off6] = state->regs[SR];
}

void setCC(lc3machine* state, unsigned char DR){
	if(state->regs[DR] < 0)
		state->cc = LC3_NEGATIVE;
	else if(state->regs[DR] == 0)
		state->cc = LC3_ZERO;
	else
		state->cc = LC3_POSITIVE;
}
void lc3_trap(lc3machine* state, unsigned char vector8)
{
	if(vector8 == HALT){
		state->halted = 1;
		state->pc--;
		//printf("Halt!\n");
	}
	else if(vector8 == PUTSP){
			unsigned short currAddr = state->regs[0];
		while(state->mem[currAddr]){
			printf("%c%c", (state->mem[currAddr]&0xff), ((state->mem[currAddr]>>8)&0xff));
			currAddr++;
		}
		
	}
	else if(vector8 == IN){
		printf("Enter char: ");
		state->regs[0] = getchar();
		//putchar(state->regs[0]);
		state->regs[0] &= 0xff;
		printf("%c\n", state->regs[0]);
	}
	else if(vector8 == PUTS){
		unsigned short currAddr = state->regs[0];
		while(state->mem[currAddr]){
			printf("%c", (char)state->mem[currAddr]);
			currAddr++;
		}
	}
	else if(vector8 == OUT){
		printf("%c", (char)state->regs[0]);
	}
	else if(vector8 == GETC){
		state->regs[0] = getchar();
		state->regs[0] &= 0xff;
	}
}

signed short sext(short offset, int len_offset){
	unsigned char sign = (offset>>(len_offset-1))&1;
	short ans = offset;
	for(int i = len_offset; i<20; i++)
		ans |= (sign<<i);
	//printf("%i\n", ans);
	return ans;
}
